/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "vehiclepositionmanage.h"
#include "filteringalgorithm.h"
#include "motionmessage.h"
#include <qdebug.h>
#include <QSettings>
#include <QVariant>
#define D_POSITION_FILTER_VEL_LIMIT_RATIO 1.5
#define D_POSITION_FILTER_VEL_ROTATE_LIMIT_RATIO 2.5
#define D_MAX_NO_FILTER_COUNT 150


VehiclePositionManage::VehiclePositionManage(int robotIdIn, MotionMessage * motionMessageIn,
                                             QString odomFileIn, QString positionConfigFileIn,
                                             double deltaTimeIn)
{
    robotId=robotIdIn;
    motionMessage=motionMessageIn;
    odomFile=odomFileIn;
    positionConfigFile=positionConfigFileIn;
    deltaTime=deltaTimeIn;
//    favoritePositionType=E_VEHICLE_POSITION_TYPE_ROS;
//    currentPositionType=E_VEHICLE_POSITION_TYPE_ROS;
//    favoritePositionType=E_VEHICLE_POSITION_TYPE_VMARK;
    currentPositionType=E_VEHICLE_POSITION_TYPE_VMARK;
    filtering_rosPosition_x=new FilteringAlgorithm;
    filtering_rosPosition_y=new FilteringAlgorithm;
    filtering_rosPosition_r=new FilteringAlgorithm;
    filtering_vmarkPosition_x=new FilteringAlgorithm;
    filtering_vmarkPosition_y=new FilteringAlgorithm;
    filtering_vmarkPosition_r=new FilteringAlgorithm;
    filtering_reflectPosition_x=new FilteringAlgorithm;
    filtering_reflectPosition_y=new FilteringAlgorithm;
    filtering_reflectPosition_r=new FilteringAlgorithm;

    maxVel_line=1.5;//todo
    maxAcc_line=0.5;
    maxVel_rotate=0.4;
    maxAcc_rotate=0.4;

    vehiclePositionFeedback_ros.position.x=0;
    vehiclePositionFeedback_ros.position.y=0;
    vehiclePositionFeedback_ros.position.rotate=0;
    vehiclePositionFeedback_ros.isUpdateSucess=false;
    vehiclePositionFeedback_ros.matchScore=10000;
    vehiclePositionFeedback_ros.maxMatchError=-1;
    vehiclePositionFeedback_vmark.position.x=0;
    vehiclePositionFeedback_vmark.position.y=0;
    vehiclePositionFeedback_vmark.position.rotate=0;
    vehiclePositionFeedback_vmark.isUpdateSucess=false;
    vehiclePositionFeedback_vmark.matchScore=10000;
    vehiclePositionFeedback_vmark.maxMatchError=-1;
    vehiclePositionFeedback_qrcode.position.x=0;
    vehiclePositionFeedback_qrcode.position.y=0;
    vehiclePositionFeedback_qrcode.position.rotate=0;
    vehiclePositionFeedback_qrcode.isUpdateSucess=false;
    vehiclePositionFeedback_qrcode.matchScore=10000;
    vehiclePositionFeedback_qrcode.maxMatchError=-1;
    vehiclePositionFeedback_cu1.position.x=0;
    vehiclePositionFeedback_cu1.position.y=0;
    vehiclePositionFeedback_cu1.position.rotate=0;
    vehiclePositionFeedback_cu1.isUpdateSucess=false;
    vehiclePositionFeedback_cu1.matchScore=10000;
    vehiclePositionFeedback_cu1.maxMatchError=-1;
    isCheckVehiclePosition_vmark=false;
    isCheckVehiclePosition_amcl=true;
    isCheckVehiclePosition_qrcode=false;
    isCheckVehiclePosition_cu1=false;

    isReflectPositionOk=true;
    isVmarkPositionOk=true;
    initialPositionNoFilterCount_ros=D_MAX_NO_FILTER_COUNT*10;
    initialPositionNoFilterCount_vmark=D_MAX_NO_FILTER_COUNT*10;
    initialPositionNoFilterCount_reflect=D_MAX_NO_FILTER_COUNT*10;


    if(1!=initialOdomFile(odomFileIn))
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8186,robotId);
    }

    if(1!=initialPositionConfigFile(positionConfigFile))
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8189,robotId);
    }



}

int VehiclePositionManage::getVehiclePositionCalibrateInfo(VehiclePosition &infoOut)
{
    infoOut.x=vehicle_x_offset;
    infoOut.y=vehicle_y_offset;
    infoOut.rotate=vehicle_r_offset;

    return 1;
}

int VehiclePositionManage::setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE typeIn, bool isCheck)
{
//    qDebug()<<"setCheckVehiclePosition type="<<typeIn<<" isCheck"<<isCheck;
    //所有不检查
    isCheckVehiclePosition_amcl=false;
    isCheckVehiclePosition_vmark=false;
    isCheckVehiclePosition_qrcode=false;
    isCheckVehiclePosition_cu1=false;


    switch(typeIn)
    {
    case E_VEHICLE_POSITION_TYPE_ROS:
    {
        isCheckVehiclePosition_amcl=isCheck;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_ENCODER_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_IMU_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_VMARK:
    {
        isCheckVehiclePosition_vmark=isCheck;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_QRCODE:
    {
        isCheckVehiclePosition_qrcode=isCheck;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_CU1:
    {
        isCheckVehiclePosition_cu1=isCheck;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_PCL_MATCH:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_UWB:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_PREDICTION:
    {

        break;
    }
    default:
    {
        return -1;
    }
    }

    return 1;

}

int VehiclePositionManage::setCheckVehiclePosition_vmark(bool isCheck)
{
    isCheckVehiclePosition_vmark=isCheck;
    return 1;
}

int VehiclePositionManage::setCheckVehiclePosition_amcl(bool isCheck)
{
    isCheckVehiclePosition_amcl=isCheck;
    return 1;
}

int VehiclePositionManage::setCheckVehiclePosition_qrcode(bool isCheck)
{
    isCheckVehiclePosition_qrcode=isCheck;
    return 1;
}

int VehiclePositionManage::calibrateVehicle(double xOffset, double yOffset, double rOffset)
{
    vehicle_x_offset=xOffset;
    vehicle_y_offset=yOffset;
    vehicle_r_offset=rOffset;
    saveVehicleOffsetToFile();
    return 1;
}

int VehiclePositionManage::getVehicleSpeedFeedback(VehicleVelocity &velocityOut)
{
    velocityOut=currentVehicleVelocity;
    return 1;
}

int VehiclePositionManage::getOdometer(double &historyOdomOut, double &historyTimeOut, double &customOdomOut,
                                       double &customeTimeOut)
{
    historyOdomOut=historyOdometer;
    customOdomOut=customOdometer;
    historyTimeOut=historyTime;
    customeTimeOut=customTime;
    return 1;
}

int VehiclePositionManage::resetCustomOdometer()
{
    customOdometer=0;
    customTime=0;
    return saveOdometerToFile();
}

int VehiclePositionManage::saveOdometerToFile()
{

    QVariant strConfigValue;
    QString firstTimeFlagStr = odomFile;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    strConfigGroup = "OdometerInfo";
    settings.beginGroup(strConfigGroup);

    //historyOdometer
    strConfigKey = "historyOdometer";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    settings.setValue(strConfigKey,QVariant(historyOdometer));

    //customOdometer
    strConfigKey = "customOdometer";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    settings.setValue(strConfigKey,QVariant(customOdometer));


    //historyTime
    strConfigKey = "historyTime";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    settings.setValue(strConfigKey,QVariant(historyTime));

    //customTime
    strConfigKey = "customTime";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    settings.setValue(strConfigKey,QVariant(customTime));




    settings.endGroup();


    qDebug() << "VehiclePositionManage::saveOdometerToFile sucess";
    return 1;


}

int VehiclePositionManage::calculateOdometerLoop()
{
    double t1=(velocityUpdataStamp.tv_nsec + 1000000000 * velocityUpdataStamp.tv_sec)/1000000000.0;
    double t2=(velocityUpdataStamp_old.tv_nsec + 1000000000 * velocityUpdataStamp_old.tv_sec)/1000000000.0;
    //时间间隔异常则返回丢失去
    double delta=t1-t2;
    if(delta>10 || delta<=0)
    {
        return -1;
    }

    double composedVelocity=sqrt(currentVehicleVelocity.x_move*currentVehicleVelocity.x_move+
                                 currentVehicleVelocity.y_move*currentVehicleVelocity.y_move);
    historyOdometer+=composedVelocity*delta;
    historyTime+=delta;
    customOdometer+=composedVelocity*delta;
    customTime+=delta;
    return 1;
}

int VehiclePositionManage::reset()
{
    vehiclePositionFeedback_ros_old=vehiclePositionFeedback_ros;
    vehiclePositionFeedback_ros_oold=vehiclePositionFeedback_ros;
    return 1;
}

int VehiclePositionManage::setCurrentVehiclePosition(VehiclePosition positionIn)
{
    //todo 进行偏差计算
}


double VehiclePositionManage::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}

int VehiclePositionManage::updateVehicleVelocity(VehicleVelocity velocityIn, timespec velocityUpdataStampIn)
{
    currentVehicleVelocity=velocityIn;
    velocityUpdataStamp_old=velocityUpdataStamp;
    velocityUpdataStamp=velocityUpdataStampIn;
    calculateOdometerLoop();
    return 1;

}

int VehiclePositionManage::changeToPositionType(E_VEHICLE_POSITION_TYPE typeIn)
{


    if(currentPositionType==typeIn)
    {
        if(E_VEHICLE_POSITION_TYPE_ROS==typeIn)
        {
            return vehiclePositionFeedback_ros.isUpdateSucess;
        }
        return 1;
    }

    qDebug()<<"VehiclePositionManage::changeToPositionType"<<typeIn;

    int tmpResult=-1;
    //等待新的成功，可能会失败,,,???
//    usleep(50000);
//    sleep(1);
    switch(typeIn)
    {
    case E_VEHICLE_POSITION_TYPE_ROS:
    {
        initialPositionNoFilterCount_ros=D_MAX_NO_FILTER_COUNT*10;//时间太短，可能开机的时候，位置不对。
        if(false==vehiclePositionFeedback_ros.isUpdateSucess)
        {
            tmpResult=-1;
        }
        else
        {
            tmpResult=1;
        }

        break;
    }
    case E_VEHICLE_POSITION_TYPE_ENCODER_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_IMU_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_VMARK:
    {
//        if(false==vehiclePositionFeedback_vmark.isUpdateSucess)//可能有线程同步问题

        initialPositionNoFilterCount_vmark=D_MAX_NO_FILTER_COUNT;
        //多次尝试
        int tmpTryCount=5;
        do
        {
            sleep(1);
            tmpTryCount--;
            qDebug()<<"change to vmark try "<<tmpTryCount<<"cu1IsUpdateSucess"<<cu1IsUpdateSucess;
        }
        while(false==vmarkIsUpdateSucess && tmpTryCount>0);
        if(false== vmarkIsUpdateSucess)
        {
            tmpResult=-1;
        }
        else
        {
            tmpResult=1;
        }
        break;

    }
    case E_VEHICLE_POSITION_TYPE_QRCODE:
    {
        if(false==qrcodeIsUpdateSucess)
        {
            tmpResult=-1;
        }
        else
        {
            tmpResult=1;
        }
        break;
    }
    case E_VEHICLE_POSITION_TYPE_CU1:
    {
        if(false==isReflectNeedInitialPosition())
        {
            qDebug()<<"false==isReflectNeedInitialPosition()";
            tmpResult=1;
            break;
        }
        initialPositionNoFilterCount_reflect=D_MAX_NO_FILTER_COUNT;
        //多次尝试
        int tmpTryCount=reflect_allow_position_change_count;
        do
        {
            sleep(1);
            tmpTryCount--;
            qDebug()<<"change to reflect try "<<tmpTryCount<<"cu1IsUpdateSucess"<<cu1IsUpdateSucess;
        }
        while(false==cu1IsUpdateSucess && tmpTryCount>0);

        if(false==cu1IsUpdateSucess)
        {
            tmpResult=-1;
        }
        else
        {
            tmpResult=1;
        }
        break;
    }
    case E_VEHICLE_POSITION_TYPE_PCL_MATCH:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_UWB:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_PREDICTION:
    {

        break;
    }
    default:
    {
        tmpResult=-1;
    }
    }

    currentPositionType=typeIn;

    return tmpResult;
}

int VehiclePositionManage::getVehiclePositionType(E_VEHICLE_POSITION_TYPE &typeOut)
{
    typeOut=currentPositionType;
    return 1;
}

int VehiclePositionManage::getVehiclePosition_default(VehiclePosition &positionReturn, E_VEHICLE_POSITION_TYPE &typeOut)
{
    PositionInfo tmpPositionReturn;
    int tmpReturn=getVehiclePosition_specified(tmpPositionReturn,currentPositionType);
    positionReturn=tmpPositionReturn.position;
    return tmpReturn;

}

int VehiclePositionManage::updateVehiclePosition(PositionInfo positionInfoIn, E_VEHICLE_POSITION_TYPE typeIn)
{
    positionUpdateMutex.lock();


    int filterBufferSize=10;
    switch(typeIn)
    {
    case E_VEHICLE_POSITION_TYPE_ROS:
    {
        double lineSpeedLimit=(fabs(currentVehicleVelocity.x_move)+fabs(currentVehicleVelocity.y_move))
                                            *ros_position_filter_line_ratio;
        double rotateSpeedLimit=fabs(currentVehicleVelocity.z_rotate)*ros_position_filter_angle_ratio;

        if(initialPositionNoFilterCount_ros>0)
        {
            if(ros_position_filter_line_ratio>0 && positionInfoIn.isUpdateSucess)//开机时可能需要很久才能定位正常．
            {
//                qDebug()<<"initialPositionNoFilterCount_ros"<<initialPositionNoFilterCount_ros;
                initialPositionNoFilterCount_ros--;//屏蔽滤波，用于测试
            }

            //
            filtering_rosPosition_x->resetAll();
            filtering_rosPosition_y->resetAll();
            filtering_rosPosition_r->resetAll();
            vehiclePositionFeedback_ros_oold=positionInfoIn;
            vehiclePositionFeedback_ros_old=positionInfoIn;
            vehiclePositionFeedback_ros=positionInfoIn;

            filtering_rosPosition_x->filterByVelocityLimit(
                                   positionInfoIn.position.x,deltaTime,lineSpeedLimit,filterBufferSize);
            filtering_rosPosition_y->filterByVelocityLimit(
                        positionInfoIn.position.y,deltaTime,lineSpeedLimit,filterBufferSize);
            filtering_rosPosition_r->filterByVelocityLimit_angle(
                        positionInfoIn.position.rotate,deltaTime,rotateSpeedLimit,filterBufferSize);


        }
        else
        {

            vehiclePositionFeedback_ros_oold=vehiclePositionFeedback_ros_old;
            vehiclePositionFeedback_ros_old=vehiclePositionFeedback_ros;
            //做滤波处理
            PositionInfo tmpPosition=positionInfoIn;
            tmpPosition.position.x=filtering_rosPosition_x->filterByVelocityLimit(
                                   positionInfoIn.position.x,deltaTime,lineSpeedLimit,filterBufferSize);
            tmpPosition.position.y=filtering_rosPosition_y->filterByVelocityLimit(
                        positionInfoIn.position.y,deltaTime,lineSpeedLimit,filterBufferSize);
            tmpPosition.position.rotate=filtering_rosPosition_r->filterByVelocityLimit_angle(
                        positionInfoIn.position.rotate,deltaTime,rotateSpeedLimit,filterBufferSize);
    //        tmpPosition.position.rotate=positionInfoIn.position.rotate;
            vehiclePositionFeedback_ros=tmpPosition;

        }


        break;
    }
    case E_VEHICLE_POSITION_TYPE_ENCODER_ODOM:
    {
        vehiclePositionFeedback_encoder_odom=positionInfoIn;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_IMU_ODOM:
    {
        vehiclePositionFeedback_imu_odom=positionInfoIn;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_VMARK:
    {
        double lineSpeedLimit=(fabs(currentVehicleVelocity.x_move)+fabs(currentVehicleVelocity.y_move))
                                            *vmark_position_filter_line_ratio;
        double rotateSpeedLimit=fabs(currentVehicleVelocity.z_rotate)*vmark_position_filter_angle_ratio;
        if(initialPositionNoFilterCount_vmark>0)
        {
            if(vmark_position_filter_line_ratio>0)
            {
                   initialPositionNoFilterCount_vmark--;//屏蔽滤波，用于测试
            }

            //
            filtering_vmarkPosition_x->resetAll();
            filtering_vmarkPosition_y->resetAll();
            filtering_vmarkPosition_r->resetAll();
            vehiclePositionFeedback_vmark_oold=positionInfoIn;
            vehiclePositionFeedback_vmark_old=positionInfoIn;
            vehiclePositionFeedback_vmark=positionInfoIn;

            filtering_vmarkPosition_x->filterByVelocityLimit(
                                   positionInfoIn.position.x,deltaTime,lineSpeedLimit,filterBufferSize);
            filtering_vmarkPosition_y->filterByVelocityLimit(
                        positionInfoIn.position.y,deltaTime,lineSpeedLimit,filterBufferSize);
            filtering_vmarkPosition_r->filterByVelocityLimit_angle(
                        positionInfoIn.position.rotate,deltaTime,rotateSpeedLimit,filterBufferSize);


            vmarkIsUpdateSucess=positionInfoIn.isUpdateSucess;
        }
        else
        {

            vehiclePositionFeedback_vmark_oold=vehiclePositionFeedback_vmark_old;
            vehiclePositionFeedback_vmark_old=vehiclePositionFeedback_vmark;

            vehiclePositionFeedback_vmark=positionInfoIn;//在这里做滤波处理不合适，误差已经耦合了，滤波效果也不行

            //做滤波处理
            PositionInfo tmpPosition=positionInfoIn;
            tmpPosition.position.x=filtering_vmarkPosition_x->filterByVelocityLimit(
                                   positionInfoIn.position.x,deltaTime,lineSpeedLimit,filterBufferSize);
            tmpPosition.position.y=filtering_vmarkPosition_y->filterByVelocityLimit(
                        positionInfoIn.position.y,deltaTime,lineSpeedLimit,filterBufferSize);
            tmpPosition.position.rotate=filtering_vmarkPosition_r->filterByVelocityLimit_angle(
                        positionInfoIn.position.rotate,deltaTime,rotateSpeedLimit,filterBufferSize);
    //        tmpPosition.position.rotate=positionInfoIn.position.rotate;
            vehiclePositionFeedback_vmark=tmpPosition;
            vmarkIsUpdateSucess=positionInfoIn.isUpdateSucess;

        }


        break;
    }
    case E_VEHICLE_POSITION_TYPE_QRCODE:
    {
        vehiclePositionFeedback_qrcode_oold=vehiclePositionFeedback_qrcode_old;
        vehiclePositionFeedback_qrcode_old=vehiclePositionFeedback_qrcode;

        vehiclePositionFeedback_qrcode=positionInfoIn;
        qrcodeIsUpdateSucess=positionInfoIn.isUpdateSucess;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_CU1:
    {
        double lineSpeedLimit=(fabs(currentVehicleVelocity.x_move)+fabs(currentVehicleVelocity.y_move))
                                            *reflect_position_filter_line_ratio;
        double rotateSpeedLimit=fabs(currentVehicleVelocity.z_rotate)*reflect_position_filter_angle_ratio;
        if(initialPositionNoFilterCount_reflect>0)
        {
            if(reflect_position_filter_line_ratio>0)
            {
                   initialPositionNoFilterCount_reflect--;//屏蔽滤波，用于测试
            }

            //
            filtering_reflectPosition_x->resetAll();
            filtering_reflectPosition_y->resetAll();
            filtering_reflectPosition_r->resetAll();

            vehiclePositionFeedback_cu1_oold=positionInfoIn;
            vehiclePositionFeedback_cu1_old=positionInfoIn;
            vehiclePositionFeedback_cu1=positionInfoIn;

            filtering_reflectPosition_x->filterByVelocityLimit(
                                   positionInfoIn.position.x,deltaTime,lineSpeedLimit,filterBufferSize);
            filtering_reflectPosition_y->filterByVelocityLimit(
                        positionInfoIn.position.y,deltaTime,lineSpeedLimit,filterBufferSize);
            filtering_reflectPosition_r->filterByVelocityLimit_angle(
                        positionInfoIn.position.rotate,deltaTime,rotateSpeedLimit,filterBufferSize);
            cu1IsUpdateSucess=positionInfoIn.isUpdateSucess;

        }
        else
        {

            vehiclePositionFeedback_cu1_oold=vehiclePositionFeedback_cu1_old;
            vehiclePositionFeedback_cu1_old=vehiclePositionFeedback_cu1;
            //做滤波处理
            PositionInfo tmpPosition=positionInfoIn;
            tmpPosition.position.x=filtering_reflectPosition_x->filterByVelocityLimit(
                                   positionInfoIn.position.x,deltaTime,lineSpeedLimit,filterBufferSize);
            tmpPosition.position.y=filtering_reflectPosition_y->filterByVelocityLimit(
                        positionInfoIn.position.y,deltaTime,lineSpeedLimit,filterBufferSize);
            tmpPosition.position.rotate=filtering_reflectPosition_r->filterByVelocityLimit_angle(
                        positionInfoIn.position.rotate,deltaTime,rotateSpeedLimit,filterBufferSize);
    //        tmpPosition.position.rotate=positionInfoIn.position.rotate;
            vehiclePositionFeedback_cu1=tmpPosition;
            cu1IsUpdateSucess=positionInfoIn.isUpdateSucess;

        }


        break;
    }
    case E_VEHICLE_POSITION_TYPE_PCL_MATCH:
    {
        vehiclePositionFeedback_pcl=positionInfoIn;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_UWB:
    {
        vehiclePositionFeedback_UWB=positionInfoIn;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_PREDICTION:
    {
        vehiclePositionFeedback_prediction=positionInfoIn;
        break;
    }
    default:
    {
        positionUpdateMutex.unlock();
        return -1;
    }
    }
    checkPositionFeedbackIsResonable(typeIn);
    positionUpdateMutex.unlock();
    return 1;

}

int VehiclePositionManage::initialVehiclePosition( E_VEHICLE_POSITION_TYPE typeIn)
{
    positionUpdateMutex.lock();

    switch(typeIn)
    {
    case E_VEHICLE_POSITION_TYPE_ROS:
    {
        initialPositionNoFilterCount_ros=D_MAX_NO_FILTER_COUNT*10;//时间太短，可能开机的时候，位置不对。


        break;
    }
    case E_VEHICLE_POSITION_TYPE_ENCODER_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_IMU_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_VMARK:
    {
        initialPositionNoFilterCount_vmark=D_MAX_NO_FILTER_COUNT;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_QRCODE:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_CU1:
    {

        initialPositionNoFilterCount_reflect=D_MAX_NO_FILTER_COUNT;


        break;
    }
    case E_VEHICLE_POSITION_TYPE_PCL_MATCH:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_UWB:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_PREDICTION:
    {

        break;
    }
    default:
    {
        positionUpdateMutex.unlock();
        return -1;
    }
    }

    positionUpdateMutex.unlock();
    return 1;
}

int VehiclePositionManage::setReflectPositionCertainity(VehiclePosition uncertainPoseIn,
                                                        E_VEHICLE_POSITION_TYPE typeIn)
{

    switch(typeIn)
    {
    case E_VEHICLE_POSITION_TYPE_ROS:
    {


        break;
    }
    case E_VEHICLE_POSITION_TYPE_ENCODER_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_IMU_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_VMARK:
    {
        vmark_uncertainPose=uncertainPoseIn;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_QRCODE:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_CU1:
    {

        reflect_uncertainPose=uncertainPoseIn;

        break;
    }
    case E_VEHICLE_POSITION_TYPE_PCL_MATCH:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_UWB:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_PREDICTION:
    {

        break;
    }
    default:
    {

        return -1;
    }
    }


    return 1;
}

int VehiclePositionManage::getReflectPositionStatus(bool &isOk)
{
    if(isCheckVehiclePosition_cu1)
    {
        isOk=isReflectPositionOk;

    }
    else if(isCheckVehiclePosition_vmark)
    {
        isOk=isReflectPositionOk;

    }
    else
    {
        isOk=true;
        return 1;
    }

    return 1;

}

bool VehiclePositionManage::isReflectNeedInitialPosition()
{
//    return true;
    if(vehiclePositionFeedback_cu1.isUpdateSucess )
    {
        if(false==vehiclePositionFeedback_ros.isUpdateSucess)
        {
            return false;
        }
        else
        {
            double tmpDistance=sqrt((vehiclePositionFeedback_ros.position.x-vehiclePositionFeedback_cu1.position.x)*
                                    (vehiclePositionFeedback_ros.position.x-vehiclePositionFeedback_cu1.position.x)+
                                    (vehiclePositionFeedback_ros.position.y-vehiclePositionFeedback_cu1.position.y)*
                                    (vehiclePositionFeedback_ros.position.y-vehiclePositionFeedback_cu1.position.y));
            double tmpAngle=fabs(angleRotationJudgement(vehiclePositionFeedback_ros.position.rotate,
                                                        vehiclePositionFeedback_cu1.position.rotate));
            if(tmpDistance<reflect_allowMaxDistanceOffsetBewteenAmcl &&tmpAngle<reflect_allowMaxAngleOffsetBewteenAmcl)
            {
                return false;
            }
            else
            {
                return true;
            }
        }
    }
    return true;
}


int VehiclePositionManage::saveVehicleOffsetToFile()
{

    QVariant strConfigValue;
    QString firstTimeFlagStr = positionConfigFile;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    strConfigGroup = "VehiclePositionOffset";
    settings.beginGroup(strConfigGroup);

    //vehicle_x_offset
    strConfigKey = "vehicle_x_offset";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    settings.setValue(strConfigKey,QVariant(vehicle_x_offset));

    //vehicle_y_offset
    strConfigKey = "vehicle_y_offset";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    settings.setValue(strConfigKey,QVariant(vehicle_y_offset));


    //vehicle_r_offset
    strConfigKey = "vehicle_r_offset";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    settings.setValue(strConfigKey,QVariant(vehicle_r_offset));


    settings.endGroup();


    qDebug() << "VehiclePositionManage::saveVehicleOffsetToFile sucess";
    return 1;
}

int VehiclePositionManage::checkPosition_pcl(PositionInfo positionInfo)
{
    if(false==positionInfo.isUpdateSucess)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8187,robotId);
        return -1;
    }
    if(maxMumPclMatchScore<positionInfo.matchScore)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8188,robotId);
        return -2;
    }

    return 1;

}

int VehiclePositionManage::checkPosition_amcl(PositionInfo positionInfo)
{
    if(false==isCheckVehiclePosition_amcl )
    {
        return 0;
    }
    static int bootCount=0;
    if(bootCount<2000)
    {
        bootCount++;
    }
    #ifdef D_USE_AMCL_ADVANCE
    static int errorCount=0;
    if(false==positionInfo.isUpdateSucess)
    {
        //伟初始化完成前，屏蔽该错误。
        if(1500<bootCount)
        {
            addMsg(ENUM_MSG_WARNING,"MotionServer","VehiclePositionManage",8190,robotId);
            return -1;
        }
    }
    if(positionInfo.matchScore<minimumAmclMatchScore)
    {
        errorCount++;
    }
    else
    {
        errorCount=0;
    }

    if(errorCount>amcl_allow_max_error_count)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8191,robotId,positionInfo.matchScore,
               minimumAmclMatchScore,amcl_allow_max_error_count);
        return -2;
    }
    #endif

    return 1;
}

int VehiclePositionManage::checkPosition_vmark(PositionInfo positionInfo)
{
//    qDebug()<<"VehiclePositionManage::checkPosition_vmark"<<isCheckVehiclePosition_vmark;
    if(false==isCheckVehiclePosition_vmark)
    {
        return 0;
    }

    static int errorCount=0;

    if(false==positionInfo.isUpdateSucess)
    {
        errorCount++;
    }
    else
    {
        errorCount=0;
    }

    if(errorCount>vmark_allow_max_error_count)
    {
        if(0==positionInfo.matchScore)
        {
            addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8192,robotId,vmark_allow_max_error_count);
        }
//        else
//        {
//            addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8193,robotId,positionInfo.matchScore);
//        }

        return -1;
    }



    if(vmark_allow_distance_tolorence<fabs(vmark_uncertainPose.x))
    {
        addMsg(ENUM_MSG_WARNING,"MotionServer","VehiclePositionManage",8200,robotId,
               1000*vmark_uncertainPose.x,1000*vmark_allow_distance_tolorence);
        isVmarkPositionOk=false;
        return 0;
    }

    if(vmark_allow_distance_tolorence<fabs(vmark_uncertainPose.y))
    {
        addMsg(ENUM_MSG_WARNING,"MotionServer","VehiclePositionManage",8200,robotId,
               1000*vmark_uncertainPose.y,1000*vmark_allow_distance_tolorence);
        isVmarkPositionOk=false;
        return 0;
    }

    if(vmark_allow_angle_tolorence<fabs(vmark_uncertainPose.rotate))
    {
        addMsg(ENUM_MSG_WARNING,"MotionServer","VehiclePositionManage",8201,robotId,
               180*vmark_uncertainPose.rotate/M_PI,180*vmark_allow_angle_tolorence/M_PI);
        isVmarkPositionOk=false;
        return 0;
    }

    isVmarkPositionOk=true;

    return 1;
}

int VehiclePositionManage::checkPosition_qrcode(PositionInfo positionInfo)
{
    if(false==isCheckVehiclePosition_qrcode )
    {
        return 0;
    }

    static int errorCount=0;
    if(false==positionInfo.isUpdateSucess)
    {
        errorCount++;
    }
    else
    {
        errorCount=0;
    }

    if(errorCount>qrcode_allow_max_error_count)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8196,robotId,qrcode_allow_max_error_count);
        return -1;
    }


    return 1;
}

int VehiclePositionManage::checkPosition_cu1(PositionInfo positionInfo)
{
    if(false==isCheckVehiclePosition_cu1 )
    {
        return 0;
    }

    static int errorCount=0;
    if(false==positionInfo.isUpdateSucess)
    {
        errorCount++;
    }
    else
    {
        errorCount=0;
    }

    if(errorCount>reflect_allow_max_error_count)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8199,robotId,reflect_allow_max_error_count);
        isReflectPositionOk=false;
        return -1;
    }



    if(reflect_allow_distance_tolorence<fabs(reflect_uncertainPose.x))
    {
        addMsg(ENUM_MSG_WARNING,"MotionServer","VehiclePositionManage",8197,robotId,
               1000*reflect_uncertainPose.x,1000*reflect_allow_distance_tolorence);
        isReflectPositionOk=false;
        return 0;
    }

    if(reflect_allow_distance_tolorence<fabs(reflect_uncertainPose.y))
    {
        addMsg(ENUM_MSG_WARNING,"MotionServer","VehiclePositionManage",8197,robotId,
               1000*reflect_uncertainPose.y,1000*reflect_allow_distance_tolorence);
        isReflectPositionOk=false;
        return 0;
    }

    if(reflect_allow_angle_tolorence<fabs(reflect_uncertainPose.rotate))
    {
        addMsg(ENUM_MSG_WARNING,"MotionServer","VehiclePositionManage",8198,robotId,
               180*reflect_uncertainPose.rotate/M_PI,180*reflect_allow_angle_tolorence/M_PI);
        isReflectPositionOk=false;
        return 0;
    }

    isReflectPositionOk=true;

    return 1;
}

int VehiclePositionManage::initialOdomFile(QString odomFileIn)
{
    QVariant strConfigValue;
    QString firstTimeFlagStr = odomFileIn;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    strConfigGroup = "OdometerInfo";
    settings.beginGroup(strConfigGroup);

    //historyOdometer
    strConfigKey = "historyOdometer";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    historyOdometer= strConfigValue.toDouble();

    //customOdometer
    strConfigKey = "customOdometer";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    customOdometer= strConfigValue.toDouble();

    //historyTime
    strConfigKey = "historyTime";
    if(false==settings.contains(strConfigKey))
    {
        return -3;
    }
    strConfigValue = settings.value(strConfigKey);
    historyTime= strConfigValue.toDouble();

    //customTime
    strConfigKey = "customTime";
    if(false==settings.contains(strConfigKey))
    {
        return -4;
    }
    strConfigValue = settings.value(strConfigKey);
    customTime= strConfigValue.toDouble();


    settings.endGroup();





    qDebug() << "VehiclePositionManage::initialOdomFile sucess";
    return 1;
}

int VehiclePositionManage::initialPositionConfigFile(QString positionConfigFileIn)
{
    QVariant strConfigValue;
    QString firstTimeFlagStr = positionConfigFileIn;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");


    strConfigGroup = "PclInfo";
    settings.beginGroup(strConfigGroup);

    //maxMumPclMatchScore
    strConfigKey = "maxMumPclMatchScore";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    maxMumPclMatchScore= strConfigValue.toDouble();

    settings.endGroup();

    strConfigGroup = "AmclInfo";
    settings.beginGroup(strConfigGroup);

    //minimumAmclMatchScore
    strConfigKey = "minimumAmclMatchScore";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    minimumAmclMatchScore= strConfigValue.toDouble();

    //amcl_allow_max_error_count
    strConfigKey = "amcl_allow_max_error_count";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    amcl_allow_max_error_count= strConfigValue.toInt();

    //isCheckVehiclePosition_amcl
    strConfigKey = "isCheckVehiclePosition_amcl";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    isCheckVehiclePosition_amcl= strConfigValue.toBool();

    //ros_position_filter_line_ratio
    strConfigKey = "ros_position_filter_line_ratio";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    ros_position_filter_line_ratio= strConfigValue.toDouble();



    //ros_position_filter_angle_ratio
    strConfigKey = "ros_position_filter_angle_ratio";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    ros_position_filter_angle_ratio= strConfigValue.toDouble();




    settings.endGroup();

    strConfigGroup = "VmarkInfo";
    settings.beginGroup(strConfigGroup);

    //vmark_allow_max_error_count
    strConfigKey = "vmark_allow_max_error_count";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vmark_allow_max_error_count= strConfigValue.toInt();

    //vmark_allow_distance_tolorence
    strConfigKey = "vmark_allow_distance_tolorence";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vmark_allow_distance_tolorence= strConfigValue.toDouble();

    //vmark_allow_angle_tolorence
    strConfigKey = "vmark_allow_angle_tolorence";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vmark_allow_angle_tolorence= strConfigValue.toDouble();
    vmark_allow_angle_tolorence=vmark_allow_angle_tolorence*M_PI/180.0;

    //vmark_position_filter_line_ratio
    strConfigKey = "vmark_position_filter_line_ratio";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vmark_position_filter_line_ratio= strConfigValue.toDouble();



    //vmark_position_filter_angle_ratio
    strConfigKey = "vmark_position_filter_angle_ratio";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vmark_position_filter_angle_ratio= strConfigValue.toDouble();



    settings.endGroup();

    strConfigGroup = "QrcodeInfo";
    settings.beginGroup(strConfigGroup);

    //qrcode_allow_max_error_count
    strConfigKey = "qrcode_allow_max_error_count";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    qrcode_allow_max_error_count= strConfigValue.toInt();

    settings.endGroup();


    strConfigGroup = "ReflectInfo";
    settings.beginGroup(strConfigGroup);

    //reflect_allow_distance_tolorence
    strConfigKey = "reflect_allow_distance_tolorence";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    reflect_allow_distance_tolorence= strConfigValue.toDouble();

    //reflect_allow_angle_tolorence
    strConfigKey = "reflect_allow_angle_tolorence";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    reflect_allow_angle_tolorence= strConfigValue.toDouble();
    reflect_allow_angle_tolorence=reflect_allow_angle_tolorence*M_PI/180.0;

    //reflect_allow_max_error_count
    strConfigKey = "reflect_allow_max_error_count";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    reflect_allow_max_error_count= strConfigValue.toInt();

    //reflect_allow_position_change_count
    strConfigKey = "reflect_allow_position_change_count";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    reflect_allow_position_change_count= strConfigValue.toInt();

    //reflect_position_filter_line_ratio
    strConfigKey = "reflect_position_filter_line_ratio";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    reflect_position_filter_line_ratio= strConfigValue.toDouble();


    //reflect_position_filter_angle_ratio
    strConfigKey = "reflect_position_filter_angle_ratio";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    reflect_position_filter_angle_ratio= strConfigValue.toDouble();

    //reflect_allowMaxDistanceOffsetBewteenAmcl
    strConfigKey = "reflect_allowMaxDistanceOffsetBewteenAmcl";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    reflect_allowMaxDistanceOffsetBewteenAmcl= strConfigValue.toDouble();


    //reflect_allowMaxAngleOffsetBewteenAmcl
    strConfigKey = "reflect_allowMaxAngleOffsetBewteenAmcl";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    reflect_allowMaxAngleOffsetBewteenAmcl= strConfigValue.toDouble();
     reflect_allowMaxAngleOffsetBewteenAmcl=reflect_allowMaxAngleOffsetBewteenAmcl*M_PI/180.0;

    settings.endGroup();



    strConfigGroup = "VehiclePositionInfo";
    settings.beginGroup(strConfigGroup);

    //currentPositionType
    strConfigKey = "currentPositionType";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    currentPositionType= (E_VEHICLE_POSITION_TYPE)strConfigValue.toInt();

    settings.endGroup();

    strConfigGroup = "VehiclePositionOffset";
    settings.beginGroup(strConfigGroup);

    //vehicle_x_offset
    strConfigKey = "vehicle_x_offset";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicle_x_offset= strConfigValue.toDouble();

    //vehicle_y_offset
    strConfigKey = "vehicle_y_offset";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicle_y_offset= strConfigValue.toDouble();

    //vehicle_r_offset
    strConfigKey = "vehicle_r_offset";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicle_r_offset= strConfigValue.toDouble();

    settings.endGroup();




    qDebug() << "VehiclePositionManage::initialPositionConfigFile sucess";
    return 1;
}

int VehiclePositionManage::checkPositionFeedbackIsResonable(E_VEHICLE_POSITION_TYPE typeIn)
{
    int tmpKey;
    switch(typeIn)
    {
    case E_VEHICLE_POSITION_TYPE_ROS:
    {
        tmpKey= checkPositionFeedbackIsResonable_specified(vehiclePositionFeedback_ros,vehiclePositionFeedback_ros_old,
                                                          vehiclePositionFeedback_ros_oold);
        break;
    }
    case E_VEHICLE_POSITION_TYPE_ENCODER_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_IMU_ODOM:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_VMARK:
    {
        tmpKey= checkPositionFeedbackIsResonable_specified(vehiclePositionFeedback_vmark,vehiclePositionFeedback_vmark_old,
                                                          vehiclePositionFeedback_vmark_oold);
        break;
    }
    case E_VEHICLE_POSITION_TYPE_QRCODE:
    {
        tmpKey= checkPositionFeedbackIsResonable_specified(vehiclePositionFeedback_qrcode,
                                                           vehiclePositionFeedback_qrcode_old,
                                                          vehiclePositionFeedback_qrcode_oold);
        break;
    }
    case E_VEHICLE_POSITION_TYPE_CU1:
    {
        tmpKey= checkPositionFeedbackIsResonable_specified(vehiclePositionFeedback_cu1,
                                                           vehiclePositionFeedback_cu1_old,
                                                          vehiclePositionFeedback_cu1_oold);
        break;
    }
    case E_VEHICLE_POSITION_TYPE_UWB:
    {

        break;
    }
    case E_VEHICLE_POSITION_TYPE_PREDICTION:
    {

        break;
    }
    default:
    {
        return -1;
    }
    }

    if(-1==tmpKey)
    {
//        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8180,robotId,typeIn);
        return -1;
    }
    if(-2==tmpKey)
    {

//        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8181,robotId,typeIn);
        return -1;
    }
    if(-3==tmpKey)
    {

//        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8182,robotId,typeIn);
        return -1;
    }
    if(-4==tmpKey)
    {

//        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8183,robotId,typeIn);
        return -1;
    }
    if(-5==tmpKey)
    {

//        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8184,robotId,typeIn);
        return -1;
    }
    if(-6==tmpKey)
    {

//        addMsg(ENUM_MSG_ERROR,"MotionServer","VehiclePositionManage",8185,robotId,typeIn);
        return -1;
    }


    return 1;
}

int VehiclePositionManage::checkPositionFeedbackIsResonable_specified(PositionInfo posNew, PositionInfo posOld,
                                                                      PositionInfo posOOld)
{
    double vx1,vx2,vy1,vy2,vr1,vr2;
    double ax1,ay1,ar1;
    double t1,t2,t3;
    t1=(posNew.matchSucessTimeStmap.tv_nsec + 1000000000 * posNew.matchSucessTimeStmap.tv_sec)/1000000000.0;
    t2=(posOld.matchSucessTimeStmap.tv_nsec + 1000000000 * posOld.matchSucessTimeStmap.tv_sec)/1000000000.0;
    t3=(posOOld.matchSucessTimeStmap.tv_nsec + 1000000000 * posOOld.matchSucessTimeStmap.tv_sec)/1000000000.0;

    if(t1==t2 || t2==t3)
    {
        return 0;
    }

    vx1=(posNew.position.x-posOld.position.x)/(t1-t2);
    vx2=(posOld.position.x-posOOld.position.x)/(t2-t3);
    ax1=(vx1-vx2)/(t1-t2);

    vy1=(posNew.position.y-posOld.position.y)/(t1-t2);
    vy2=(posOld.position.y-posOOld.position.y)/(t2-t3);
    ay1=(vy1-vy2)/(t1-t2);

    vr1=(posNew.position.rotate-posOld.position.rotate)/(t1-t2);
    vr2=(posOld.position.rotate-posOOld.position.rotate)/(t2-t3);
    ar1=(vr1-vr2)/(t1-t2);

    if(fabs(vx1)>maxVel_line || fabs(vx2)>maxVel_line )
    {
//        qDebug()<<"velocity translation exceed error";
        return -1;
    }
    if( fabs(vy1)>maxVel_line || fabs(vy2)>maxVel_line)
    {
//        qDebug()<<"velocity translation exceed error";
        return -2;
    }

    if(fabs(vr1)>maxVel_rotate ||fabs(vr2)>maxVel_rotate)
    {
//        qDebug()<<"velocity rotate exceed error";
        return -3;
    }

    if(fabs(ax1)>maxAcc_line  )
    {
//        qDebug()<<"acc translation exceed error";
        return -4;
    }

    if(fabs(ay1)>maxAcc_line )
    {
//        qDebug()<<"acc translation exceed error";
        return -5;
    }

    if(fabs(ar1)>maxAcc_line  )
    {
//        qDebug()<<"acc rotate exceed error";
        return -6;
    }

    return 1;
}

int VehiclePositionManage::getVehiclePosition_default(PositionInfo &positionReturn,E_VEHICLE_POSITION_TYPE &typeOut)
{
    typeOut=currentPositionType;
    return getVehiclePosition_specified(positionReturn,currentPositionType);
}

int VehiclePositionManage::getVehiclePosition_specified(PositionInfo &positionReturn, E_VEHICLE_POSITION_TYPE typeIn)
{
    switch(typeIn)
    {
    case E_VEHICLE_POSITION_TYPE_ROS:
    {
        positionReturn=vehiclePositionFeedback_ros;
//        checkPosition_amcl(vehiclePositionFeedback_ros);
        break;
    }
    case E_VEHICLE_POSITION_TYPE_ENCODER_ODOM:
    {
        positionReturn=vehiclePositionFeedback_encoder_odom;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_IMU_ODOM:
    {
        positionReturn=vehiclePositionFeedback_imu_odom;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_VMARK:
    {
        positionReturn=vehiclePositionFeedback_vmark;
        checkPosition_vmark(vehiclePositionFeedback_vmark);
        break;
    }
    case E_VEHICLE_POSITION_TYPE_CU1:
    {
        positionReturn=vehiclePositionFeedback_cu1;
        checkPosition_cu1(vehiclePositionFeedback_cu1);
        break;
    }
    case E_VEHICLE_POSITION_TYPE_QRCODE:
    {
        positionReturn=vehiclePositionFeedback_qrcode;
        checkPosition_qrcode(vehiclePositionFeedback_qrcode);
        break;
    }
    case E_VEHICLE_POSITION_TYPE_PCL_MATCH:
    {
        positionReturn=vehiclePositionFeedback_pcl;
        checkPosition_pcl(positionReturn);
        break;
    }

    case E_VEHICLE_POSITION_TYPE_UWB:
    {
        positionReturn=vehiclePositionFeedback_UWB;
        break;
    }
    case E_VEHICLE_POSITION_TYPE_PREDICTION:
    {
        positionReturn=vehiclePositionFeedback_prediction;
        break;
    }

    default:
    {
        return -1;
    }
    }

    positionReturn.position.x+=vehicle_x_offset;
    positionReturn.position.y+=vehicle_y_offset;
    positionReturn.position.rotate+=vehicle_r_offset;

    return 1;
}

int VehiclePositionManage::calculatePreditionPosition()
{

}



void VehiclePositionManage::addMsg(int messageLevel, std::string componentName, std::string messageType
                              , int messageCode, int robotIdIn
                              , int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 8201:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",VMARK定位，AGV角度误差(%1degree)超过阈值(%2degree)！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8200:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",VMARK定位，AGV距离误差(%1mm)超过阈值(%2mm)！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8199:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",反光标记定位，失败次数超过阈值(%1)！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8198:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",反光标记定位，AGV角度误差(%1degree)超过阈值(%2degree)！").
                arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8197:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",反光标记定位，AGV距离误差(%1mm)超过阈值(%2mm)！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }

    case 8196:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",没有探测到二维码标记！允许失败次数%1。1)检查二维码是否在摄像头可见范围内？2)摄像头连线是否正常？３)二维码是否污损？").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8195:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",CU1位置获取失败）！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8194:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",二维码位置获取失败）！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8193:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",探测到%1个v槽，请重新设置laserType限制雷达范围！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8192:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",超过允许失败次数(%1)都没有探测到v槽！。1)检查V槽是否在雷达可见范围内？2)运动路径是否合理？3)ｖmark类型设置是否正确？4)laserType雷达范围是否设置正确？").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8191:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",amcl位置定位失败,实际分数%1小于允许分数%2！允许失败次数%3")
                .arg(parameter1).arg(parameter2)
                .arg(parameter3);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8190:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",amcl位置获取失败）！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8189:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",positionConfig.ini文件异常！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8188:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",pcl位置获取失败(位置匹配分数过低）！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8187:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",pcl位置获取失败(计算失败）！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8186:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",里程数记录文件异常！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8185:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",定位r加速度反馈异常，定位类型＝%1！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8184:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",定位y加速度反馈异常，定位类型＝%1！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8183:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",定位x加速度反馈异常，定位类型＝%1！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8182:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",定位r旋转速度反馈异常，定位类型＝%1！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8181:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",定位y平移速度反馈异常，定位类型＝%1！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8180:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",定位ｘ平移速度反馈异常，定位类型＝%1！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }



        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
